#include "publisher/publisher.h"

namespace cartographer {

Publisher::Publisher(const int& trajectory_id) {
 	landmarks_.clear();
 	poses_.clear();
 	gt_poses_.clear();
 	odometry_poses_.clear();
 	current_landmarks_.clear();
 	predict_poses2d_.clear();
 	predict_poses3d_.clear();
 	filter_landmarks_.clear();

 	trajectory_id_ = trajectory_id;

}

void Publisher::PublishOriginPointCloud(const std::vector<float>& x_coords, const std::vector<float>& y_coords) {
	current_landmarks_.clear();
	for(int i = 0; i < x_coords.size(); i++) {
        Eigen::Vector3f point = Eigen::Vector3f(x_coords[i], y_coords[i], 0.0);
        //std::cout << " point : " << point << std::endl;
 		current_landmarks_.push_back(point);
 	}
}

void Publisher::PublishFilterPointCloud(const sensor::PointCloud& accumulated_semantics_in_world, const transform::Rigid3d& pose_estimated_3d)  {
	std::lock_guard<std::mutex> lock(filter_point_cloud_mutex_); 
	filter_landmarks_.clear();
    sensor::PointCloud filter_point_clouds = sensor::TransformPointCloud3D(accumulated_semantics_in_world, pose_estimated_3d);

	for(int i = 0; i < filter_point_clouds.points().size(); i++) {
        Eigen::Vector3f point = filter_point_clouds.points()[i].position;
        
 		filter_landmarks_.push_back(point);
 	}
}

void Publisher::PublishTrackingPointCloud(const sensor::PointCloud& accumulated_semantics_in_world) {

	for(int i = 0; i < accumulated_semantics_in_world.points().size(); i++) {
        Eigen::Vector3f point = accumulated_semantics_in_world.points()[i].position;
        
        //std::cout << " point : " << point << std::endl;

 		landmarks_.push_back(point);
 	}
}

void Publisher::PublishTrackingPose(const std::unique_ptr<transform::Rigid2d>& world_pose) {
	Eigen::Matrix<double, 2, 1> pose2d = world_pose->translation();

	Eigen::Vector3d pose(pose2d(0, 0), pose2d(1, 0), world_pose->normalized_angle());
	poses_.push_back(pose);
}

void Publisher::PublishGtPose(const Eigen::Vector3d& gt_world_pose) {
	gt_poses_.push_back(gt_world_pose);
}

void Publisher::PublishOdometryPose(const Eigen::Vector3d& world_pose) {
    std::lock_guard<std::mutex> lock(odometry_mutex_); 
	odometry_poses_.push_back(world_pose);
}    

void Publisher::PublishCurrentPointCloud(const sensor::PointCloud& current_semantics) {
	current_landmarks_.clear();
	for(int i = 0; i < current_semantics.points().size(); i++) {
        Eigen::Vector3f point = current_semantics.points()[i].position;
        
        //std::cout << " point : " << point << std::endl;
 		current_landmarks_.push_back(point);
 	}
}

void Publisher::PublishPredictPose2D(const transform::Rigid2d& world_pose) {
	Eigen::Matrix<double, 2, 1> pose2d = world_pose.translation();

	Eigen::Vector3d pose(pose2d(0, 0), pose2d(1, 0), world_pose.normalized_angle());
	
	std::lock_guard<std::mutex> lock(predict_pose2d_mutex_); 
	predict_poses2d_.push_back(pose);
}

void Publisher::PublishPredictPose3D(const transform::Rigid3d& world_pose) {
	Eigen::Matrix<double, 3, 1> pose3d = world_pose.translation();

	Eigen::Vector3d pose(pose3d(0, 0), pose3d(1, 0), pose3d(2, 0));

	std::lock_guard<std::mutex> lock(predict_pose3d_mutex_); 
	predict_poses3d_.push_back(pose);
}

// void Publisher::PublishAllPoses(std::vector<Pose>& poses) {
// 	for(int i = 0; i < poses.size(); i++) {
// 		Eigen::Vector3d pose(poses[i].x, poses[i].y, poses[i].t);
// 		all_poses_.push_back(pose);
// 	}
// }

void Publisher::PublishLandmarkPosesList(const std::map<std::string, transform::Rigid3d>& landmark_poses) {
	landmark_poses_ = landmark_poses;
}

std::vector<Eigen::Vector3f> Publisher::get_landmarks() {
	return landmarks_;
}

std::vector<Eigen::Vector3d> Publisher::get_poses() {
	return poses_;
}

std::vector<Eigen::Vector3d> Publisher::get_gt_poses() {
	return gt_poses_;
}

std::vector<Eigen::Vector3d> Publisher::get_odometry_poses() {
	std::lock_guard<std::mutex> lock(odometry_mutex_); 
	return odometry_poses_;
}

std::vector<Eigen::Vector3f> Publisher::get_current_landmarks() {
	return current_landmarks_;
}

std::vector<Eigen::Vector3d> Publisher::get_predict_poses2d() {
	std::lock_guard<std::mutex> lock(predict_pose2d_mutex_); 
	return predict_poses2d_;
}

std::vector<Eigen::Vector3d> Publisher::get_predict_poses3d() {
	std::lock_guard<std::mutex> lock(predict_pose3d_mutex_); 
	return predict_poses3d_;
}

std::vector<Eigen::Vector3f> Publisher::get_filter_landmarks() {
	std::lock_guard<std::mutex> lock(filter_point_cloud_mutex_); 
	return filter_landmarks_;
}

std::vector<Eigen::Vector3d> Publisher::get_submap_landmarks() {
	std::vector<Eigen::Vector3d> submap_landmarks;

	for (const auto& landmark : landmark_poses_) {
		submap_landmarks.push_back(landmark.second.translation()); 
	}
	//std::cout << " submap_landmarks : " << submap_landmarks.size() << std::endl;
	return submap_landmarks;
}

std::vector<Eigen::Vector3d> Publisher::get_all_poses() {
	return all_poses_;	
}

} // namespace cartographer
